#ifndef _MOTION_PLAN_H
#define _MOTION_PLAN_H

#include <string.h>
#include <vector>
#include <math.h>
#include "boat_datadef.h"

// 轨迹跟踪参数结构体
typedef struct
{
    int DotNum;                 // 总航点数量
    int TrackNum;               // 当前跟踪的航点索引
    int ReturnFlag;             // 任务完成标志
    int TrackFlag;              // 跟踪状态标志
    int FirstFlag;              // 首次跟踪标志
    
    float eta;                  // 跟踪增益
    float switchlen;            // 切换距离阈值 (米)
    float setVel;               // 设定速度 (m/s)
    float maxSpeed;             // 最大速度限制 (m/s)
    float minSpeedFactor;       // 最小速度因子
    
    float firstphi;             // 初始航向角
    float TargetPos[2];         // 当前目标位置 [lat, lon]
    float TrajectoryUnit[128][2]; // 轨迹单位向量 [东分量, 北分量]
} TrackingPlanParam;

// 控制模式标志结构体
typedef struct
{
    uint8_t flash_flag;         // 命令刷新标志
    uint8_t model;              // 控制模式
} flash_model_data;

class MotionPlan
{
public:
    MotionPlan();
    ~MotionPlan() = default;
    
    // 公共成员变量
    flash_model_data actionModelData;
    actuator_control actionActuatorControl;
    headvel_control actionHeadvelControl;
    TrackingPlanParam dotTrackingParam;
    
    // 主要接口函数
    bool TrajectoryGet(const mission_control& target);
    void TrajectoryTrackingDirect(const TransState& boatstate);
    
    // 工具函数
    void ResetTracking();
    bool IsTrackingComplete() const;
    float GetDistanceToTarget(const TransState& boatstate) const;
    float GetCurrentTargetSpeed() const;
    int GetCurrentWaypointIndex() const;
    int GetTotalWaypoints() const;
    
    // 设置参数
    void SetTrackingParameters(float eta, float switch_distance, float max_speed, float default_speed, float min_speed_factor);

private:
    mission_control tracking_task_;
    int WaitTime;               // 等待时间
    int WaitTimeNum;            // 等待时间计数
    int WaitFlag;               // 等待标志
    
    // 私有辅助函数
    void updateTargetPosition();
    void calculateControlCommand(const TransState& boatstate);
    bool checkWaypointReached(const TransState& boatstate);
    void advanceToNextWaypoint();
    double calculateGeoDistance(double lat1, double lon1, double lat2, double lon2) const;
    double calculateGeoBearing(double lat1, double lon1, double lat2, double lon2) const;
    double normalizeAngle(double angle) const;
};

#endif // _MOTION_PLAN_H